#include <outputdata.h>
#include "bsp_pid_assistant.h"
#include "usart.h"
#include "string.h"
#include "stdio.h"
#include "kalman.h"
#include "bsp_control.h"

/**
  * @brief  PID调试助手数据初始化。
  * @param  data：需要发送的数据。
  * @param  Cmd_Length：数据的长度。
  * @retval 无。
  */
static _cui_temp Data_Head;

void PID_Assistant_Init(void) {
  Data_Head.ui = FRAME_HEADER;    //用于数据接收处理

  PID_Struct_Vertical.PID_Target_Val = 300;
  PID_Struct_Vertical.PID_Actual_Val = 254;

  PID_Struct_Velocity.PID_Target_Val = 200;
  PID_Struct_Velocity.PID_Actual_Val = 154;

  UPLOAD_DATA_TO_PID_Assistant(&PID_Struct_Vertical);
  UPLOAD_DATA_TO_TARGET_Assistant(&PID_Struct_Vertical, SEND_TARGET_CMD, PID_Struct_Vertical.PID_Target_Val);
  UPLOAD_DATA_TO_TARGET_Assistant(&PID_Struct_Vertical, SEND_PERIOD_CMD, 50);

  UPLOAD_DATA_TO_PID_Assistant(&PID_Struct_Velocity);
  UPLOAD_DATA_TO_TARGET_Assistant(&PID_Struct_Velocity, SEND_TARGET_CMD, PID_Struct_Velocity.PID_Target_Val);
  UPLOAD_DATA_TO_TARGET_Assistant(&PID_Struct_Velocity, SEND_PERIOD_CMD, 50);


}


/**
  * @brief  串口发送数据。
  * @param  data：需要发送的数据。
  * @param  Cmd_Length：数据的长度。
  * @retval 无。
  */
static void USART_UPLOAD_DATA(void *data, uint8_t Cmd_Length) {
  unsigned char Check_bit;    //校验位。
  unsigned char *str = (unsigned char *) data;   //将结构体内存内容转换成字符串数据。
  unsigned char Cmd_data[50];     //用于储存打包好的数据帖，最大字符容量为50个字符。


  /* 打包成数据帖 */
  {
    for (int i = 0; i < 5; ++i) {
      Cmd_data[i] = *(str + i);
      Check_bit += Cmd_data[i];
    }
    for (int i = 0; i < 4; ++i) {
      Cmd_data[5 + i] = *(str + 8 + i);
      Check_bit += Cmd_data[5 + i];
    }
    Cmd_data[9] = *(str + 12);
    Check_bit += Cmd_data[9];

    switch (Cmd_Length) {
      case DATA_PID_DATA_Length:
        for (int i = 0; i < 12; i++) {
          Cmd_data[10 + i] = *(str + 16 + i);
          Check_bit += Cmd_data[10 + i];
        }
        Cmd_data[22] = Check_bit;
        break;
      case DATA_TARGET_DATA_Length:
        for (int i = 0; i < 4; i++) {
          Cmd_data[10 + i] = *(str + 16 + i);
          Check_bit += Cmd_data[10 + i];
        }
        Cmd_data[14] = Check_bit;
        break;
      case DATA_CMD_DATA_Length:
        Cmd_data[10] = Check_bit;
        break;
    }
  }

  HAL_UART_Transmit(&huart3, Cmd_data, Cmd_Length, 0xFFFFF);
}


/**
  * @brief  设备向上位机上传PID系数。
  * @param  PID_Struct：PID结构体。
  * @retval 无。
  */
static DATA_PID_RTx Send_PIDStructure;

void UPLOAD_DATA_TO_PID_Assistant(_PID_Typedef *PID_Struct) {
  Send_PIDStructure.DATA_Head.ui = FRAME_HEADER;
  Send_PIDStructure.DATA_Channel = PID_Struct->PID_Channel_Val;
  Send_PIDStructure.DATA_Length.ui = DATA_PID_DATA_Length;
  Send_PIDStructure.DATA_Command = SEND_P_I_D_CMD;
  Send_PIDStructure.PID_P.f = PID_Struct->PID_P;
  Send_PIDStructure.PID_I.f = PID_Struct->PID_I;
  Send_PIDStructure.PID_D.f = PID_Struct->PID_D;
  Send_PIDStructure.DATA_Check_Bit = 0x11;

  USART_UPLOAD_DATA(&Send_PIDStructure, DATA_PID_DATA_Length);
}

/**
  * @brief  设备向上位机上传一个目标值系数。
  * @param  value：目标值。
  * @param  value：目标值。
  * @param  Cmd：指令类型。
  * @retval 无。
  */
static DATA_TARGET_RTx Send_TARGETStructure;

void UPLOAD_DATA_TO_TARGET_Assistant(_PID_Typedef *PID_Struct, uint8_t Cmd, int value) {
  Send_TARGETStructure.DATA_Head.ui = FRAME_HEADER;
  Send_TARGETStructure.DATA_Channel = PID_Struct->PID_Channel_Val;
  Send_TARGETStructure.DATA_Length.ui = DATA_TARGET_DATA_Length;
  Send_TARGETStructure.DATA_Command = Cmd;
  Send_TARGETStructure.DATA_Value.i = value;
  Send_TARGETStructure.DATA_Check_Bit = 0x11;

  USART_UPLOAD_DATA(&Send_TARGETStructure, DATA_TARGET_DATA_Length);
}

/**
  * @brief  设备向上位机上传指令。
  * @param  Cmd：指令类型。
  * @retval 无。
  */
static DATA_CMD_RTx Send_CMDStructure;

void UPLOAD_DATA_TO_CMD_Assistant(uint8_t Cmd) {
  Send_CMDStructure.DATA_Head.ui = FRAME_HEADER;
  Send_CMDStructure.DATA_Channel = CURVES_CH1;
  Send_CMDStructure.DATA_Length.ui = DATA_TARGET_DATA_Length;
  Send_CMDStructure.DATA_Command = Cmd;
  Send_CMDStructure.DATA_Check_Bit = 0x11;

  USART_UPLOAD_DATA(&Send_CMDStructure, DATA_CMD_DATA_Length);
}


/**
  * @brief  处理上位机发送的指令。
  * @param  无。
  * @retval 无。
  */
void Receive_Data_Process(void) {
  unsigned char Check_bit = 0;    //校验位。
  unsigned char Channel = 0;    //通道数。
  unsigned char Cmd = CMD_NONE;   //指令类型（默认空指令）。


  /* 计算校验位 */
  for (int i = 0; i < RxBuffer_Cnt - 1; i++) {
    Check_bit += RxBuffer[i];
  }
//  HAL_UART_Transmit(&huart1, RxBuffer, RxBuffer_Cnt, 0xFFFFF);

  /* 判断数据包是否正确 */
  if ((strstr((char *) RxBuffer, (char *) Data_Head.ch) != NULL) && (RxBuffer[RxBuffer_Cnt - 1] == Check_bit)) {
//    printf("找到数据\r\n");
    Cmd = RxBuffer[9];
    Channel = RxBuffer[4];

    switch (Cmd) {
      case SET_P_I_D_CMD:   //处理上位机发送的PID系数
      {
        _cf_temp pid_p, pid_i, pid_d;
        pid_p.ch[0] = RxBuffer[10];
        pid_p.ch[1] = RxBuffer[11];
        pid_p.ch[2] = RxBuffer[12];
        pid_p.ch[3] = RxBuffer[13];
        pid_i.ch[0] = RxBuffer[14];
        pid_i.ch[1] = RxBuffer[15];
        pid_i.ch[2] = RxBuffer[16];
        pid_i.ch[3] = RxBuffer[17];
        pid_d.ch[0] = RxBuffer[18];
        pid_d.ch[1] = RxBuffer[19];
        pid_d.ch[2] = RxBuffer[20];
        pid_d.ch[3] = RxBuffer[21];
//        printf("成功设置CURVES_CH%dPID参数\r\nPID_P:%2f\r\nPID_I:%2f\r\nPID_D:%2f\r\n",Channel,pid_p.f,pid_i.f,pid_d.f);
        sprintf(SendBuffer, "成功设置CURVES_CH%dPID参数\r\nPID_P:%2f\r\nPID_I:%2f\r\nPID_D:%2f\r\n", Channel, pid_p.f,
                pid_i.f, pid_d.f);
        HAL_UART_Transmit(&huart3, SendBuffer, strlen(SendBuffer), HAL_MAX_DELAY);

//        OutData_test();
        if (Channel == CURVES_CH1) {
          SET_PID_Param(&PID_Struct_Vertical, pid_p.f, pid_i.f, pid_d.f);
        } else if (Channel == CURVES_CH2) {
          SET_PID_Param(&PID_Struct_Velocity, pid_p.f, pid_i.f, pid_d.f);
        } else if (Channel == CURVES_CH3) {
          SET_PID_Param(&PID_Struct_Turn, pid_p.f, pid_i.f, pid_d.f);
        }

        break;
      }
      case SET_TARGET_INT_CMD:    //处理上位机发送的目标值
      {
        _ci_temp target_val;
        target_val.ch[0] = RxBuffer[10];
        target_val.ch[1] = RxBuffer[11];
        target_val.ch[2] = RxBuffer[12];
        target_val.ch[3] = RxBuffer[13];

        sprintf(SendBuffer, "Channel:%d\tTarget_Value：%d\r\n", Channel, target_val.i);
        HAL_UART_Transmit(&huart3, SendBuffer, strlen(SendBuffer), HAL_MAX_DELAY);
        if (Channel == CURVES_CH1) {
          SET_PID_Target_Val(&PID_Struct_Vertical, target_val.i);
        } else if (Channel == CURVES_CH2) {
          SET_PID_Target_Val(&PID_Struct_Velocity, target_val.i);
        } else if (Channel == CURVES_CH3) {
          SET_PID_Target_Val(&PID_Struct_Turn, target_val.i);
        }


        break;
      }
      case SET_TARGET_FLOAT_CMD:    //处理上位机发送的目标值
      {
        _cf_temp target_val;
        target_val.ch[0] = RxBuffer[10];
        target_val.ch[1] = RxBuffer[11];
        target_val.ch[2] = RxBuffer[12];
        target_val.ch[3] = RxBuffer[13];

        sprintf(SendBuffer, "Channel:%d\tTarget_Value：%f\r\n", Channel, target_val.f);
        HAL_UART_Transmit(&huart3, SendBuffer, strlen(SendBuffer), HAL_MAX_DELAY);
        if (Channel == CURVES_CH1) {
          SET_PID_Target_Val(&PID_Struct_Vertical, target_val.f);
        } else if (Channel == CURVES_CH2) {
          SET_PID_Target_Val(&PID_Struct_Velocity, target_val.f);
        } else if (Channel == CURVES_CH3) {
          SET_PID_Target_Val(&PID_Struct_Turn, target_val.f);
        }


        break;
      }
      case SET_TARGET_Kalman_Q: {
        _cf_temp target_val;
        target_val.ch[0] = RxBuffer[10];
        target_val.ch[1] = RxBuffer[11];
        target_val.ch[2] = RxBuffer[12];
        target_val.ch[3] = RxBuffer[13];
        Kalman_Q = target_val.f;
        printf("Kalman_Q：%f\r\n", target_val.f);
        break;
      }
      case SET_TARGET_Kalman_R: {
        _cf_temp target_val;
        target_val.ch[0] = RxBuffer[10];
        target_val.ch[1] = RxBuffer[11];
        target_val.ch[2] = RxBuffer[12];
        target_val.ch[3] = RxBuffer[13];
        Kalman_R = target_val.f;
        printf("Kalman_R：%f\r\n", target_val.f);
        break;
      }
      case RESET_CMD:     //处理上位机发送的复位指令
      {
        printf("Reset\r\n");
        HAL_NVIC_SystemReset();
        break;
      }
      case STOP_CMD:    //处理上位机发送的停止指令
      {
        printf("STOP_CMD\r\n");
        break;
      }
      case START_CMD:     //处理上位机发送的启动指令
      {
        printf("START_CMD\r\n");
        break;
      }
      case SET_PERIOD_CMD:     //处理上位机发送周期值
      {
        printf("SET_PERIOD_CMD\r\n");
        break;
      }
    }
  }
}


/**
* @brief  串口DMA接收上位机发送的指令中断执行函数。
  * @param  huart：串口配置结构体。
  * @param  hdma_usart_rx：用于 USART 接收数据的 DMA 处理结构体指针。
  * @retval 无。
  */
void USARTx_R_T_C_Handler_Part(UART_HandleTypeDef *huart, DMA_HandleTypeDef *hdma_usart_rx) {
  uint8_t USART_SR_IDLE_BIT = 0;
  uint16_t temp;

  //获取IDLE标志位
  USART_SR_IDLE_BIT = __HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE);
  if (USART_SR_IDLE_BIT != RESET) {
    //清除标志位
    __HAL_UART_CLEAR_IDLEFLAG(huart);
    //停止DMA传输，防止干扰
    HAL_UART_DMAStop(huart);
    // 获取DMA中未传输的数据个数
    temp = __HAL_DMA_GET_COUNTER(hdma_usart_rx);
    RxBuffer_Cnt = RXBUFFERSIZE - temp;
//    printf("recv %d characters:\r\n%s\r\n",RxBuffer_Cnt, RxBuffer);
//      HAL_UART_Transmit(huart,RxBuffer,RxBuffer_Cnt,HAL_MAX_DELAY);
    Receive_Data_Process();
//    Receive_Data_Process();
    memset(RxBuffer, 0, RxBuffer_Cnt);
    //重新打开DMA接收
    HAL_UART_Receive_DMA(huart, RxBuffer, RXBUFFERSIZE);
  }
}






